import sys
import os
import time
import math
from PyQt5.QtWidgets import QApplication, QPushButton, QWidget, \
QVBoxLayout,QHBoxLayout,QLineEdit,QLabel,QMainWindow,\
QGraphicsView, QGraphicsScene, QGraphicsPixmapItem
from PyQt5.QtGui import QPixmap
from PyQt5.QtCore import QSize,QThread, pyqtSignal
from PyQt5.QtCore import Qt
from PyQt5.QtCore import QTimer
import rclpy
from rclpy.node import Node

from std_msgs.msg import String,Bool,Int32
from std_srvs.srv import SetBool
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from tf_transformations import euler_from_quaternion
from tf2_ros import TransformException, Buffer, TransformListener
from mower_msgs.srv import MapNameSrv,MapSlamSrv,AddPointPose,\
    SimpleSnakePathPreview,SimpleSnakePathSave,SimpleSnakePathList,\
    TaskPath
from mower_msgs.msg import MapNameMsg,MapSlamMsg

from pynput import keyboard
from geometry_msgs.msg import Twist


from .mower_panel_ui import Ui_MainWindow


class Simulator(Node):
    def __init__(self):
        super().__init__('panel_node')

        # 初始化参数
        self.InitParams()
        
        # 配置主界面
        self.ui = Ui_MainWindow()
        self.winMain=QMainWindow()
        self.ui.setupUi(self.winMain)
        self.winMain.show()

        # 配置后台键盘钩子
        self.kb2cmd=Keyboard2CmdVel(self,self.ui)

        self.ConfigPanel()
        self.ConfigROS()
        # self.ConfigTimer()
        # self.RefreshMaplist()

        # 定期调用 rclpy.spin_once() 保证 ROS 2 回调的正常执行
        self.timer = QTimer()
        self.timer.timeout.connect(self.spin_once)
        self.timer.start(100)  # 每 100ms 调用一次 spin_once

    def spin_once(self):
        rclpy.spin_once(self, timeout_sec=0.1)

    def InitParams(self):
        self.declare_parameter('user_config', '/home/robot/dingtek_mower/user_config')
        self.user_config_param = self.get_parameter('user_config').get_parameter_value().string_value        # 获取参数
        self.map_database_path = os.path.join(self.user_config_param, 'map_database')

        self.scale_factor = 1.0  # 初始化缩放比例
        self.max_scale_factor = 5.0  # 最大缩放比例
        self.min_scale_factor = 0.1  # 最小缩放比例
        
        
    def ConfigPanel(self):

        # 配置
        # 是否开启键盘监听
        self.ui.checkBox.setChecked(True)
        self.ui.checkBox.clicked.connect(lambda state: self.on_checkbox_clicked(self.ui.checkBox.isChecked()))
        
        # 配置按钮 wsad # 没有点击事件，只有按钮按下弹起状态
        # self.ui.pushButton.clicked.connect(self.on_pushButton_clicked) # w
        # self.ui.pushButton_4.clicked.connect(self.on_pushButton_clicked) # s
        # self.ui.pushButton_2.clicked.connect(self.on_pushButton_clicked) # a
        # self.ui.pushButton_3.clicked.connect(self.on_pushButton_clicked) # d

        # 禁止方向按键切换标签
        self.ui.tabWidget.setFocusPolicy(Qt.NoFocus)
        self.ui.tabWidget_2.setFocusPolicy(Qt.NoFocus)

        # 配置速度值
        # self.ui.angularLCD.display(self.ui.angularSlider.value()/100.0)
        # self.ui.linearLCD.display(self.ui.linearSlider.value()/100.0)
        self.ui.angularLCD.display(self.kb2cmd.angular_Vel)
        self.ui.linearLCD.display(self.kb2cmd.linear_Vel)
        # 配置滑块
        self.ui.angularSlider.valueChanged.connect(lambda v: self.SetSpd("angular",v))
        self.ui.linearSlider.valueChanged.connect(lambda v: self.SetSpd("linear",v))
        self.ui.angularSlider.setValue(int(self.kb2cmd.angular_Vel*100))
        self.ui.linearSlider.setValue(int(self.kb2cmd.linear_Vel*100))

        # 刀盘按钮
        self.ui.radioButton.clicked.connect(lambda state: self.cutterhead_ctl_publish_message(True))
        self.ui.radioButton_2.clicked.connect(lambda state: self.cutterhead_ctl_publish_message(False))

        # 地图
        ## 开始新建地图
        self.ui.pushButton_5.clicked.connect(self.on_pushButton_5_clicked)
        ## 取消新建地图
        self.ui.pushButton_6.clicked.connect(self.on_pushButton_6_clicked)
        ## 保存新建地图
        self.ui.pushButton_7.clicked.connect(self.on_pushButton_7_clicked)
        ## 地图列表
        self.ui.comboBox.activated.connect(self.on_activated)
        self.ui.comboBox.currentTextChanged.connect(self.on_comboBox_currentTextChanged)
        ## 删除地图
        self.ui.pushButton_12.clicked.connect(self.on_pushButton_12_clicked)
        ## 加载地图
        self.ui.pushButton_8.clicked.connect(self.on_pushButton_8_clicked)

        # 日志
        self.ui.pushButton_13.clicked.connect(self.on_pushButton_13_clicked) # 清空

        # 简单蛇形路径（对角线）
        ## 添加对角线点（1）（2）
        self.ui.pushButton_11.clicked.connect(self.on_pushButton_11_clicked)
        ## 清除记录
        self.ui.pushButton_16.clicked.connect(self.on_pushButton_16_clicked) 
        ## 预览
        self.ui.pushButton_26.clicked.connect(self.on_pushButton_26_clicked)  
        ## 保存
        self.ui.pushButton_27.clicked.connect(self.on_pushButton_27_clicked)
        ## 列表
        self.ui.comboBox_2.activated.connect(self.on_activated_2)
        ## 沿路经运动
        self.ui.pushButton_14.clicked.connect(self.on_pushButton_14_clicked)
        ## 取消沿路经运动
        self.ui.pushButton_17.clicked.connect(self.on_pushButton_17_clicked)
        ## 列表路径预览
        self.ui.pushButton_15.clicked.connect(self.on_pushButton_15_clicked)

    
    def ConfigROS(self):

        # 航向
        self.imu_orientation = None 
        self.odom_combined_orientation = None
        self.odom_local_orientation = None
        # 机器人姿态
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0
        # frame
        self.map_frame = 'map' # odom_combined
        # self.map_frame = 'odom_combined' # odom_combined
        self.base_frame = 'base_link'
        # GPS 信号
        self.fix_type = 1

        # 监听
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # 刀盘
        self.cutterhead_ctl_pub = self.create_publisher(Bool, 'cutterhead_ctl', 1)
        # 地图
        ## 新建地图
        self.map_create_slam_client = self.create_client(MapSlamSrv,'map_create_slam')
        ## 取消新建地图
        self.map_create_cancel_slam_client = self.create_client(MapSlamSrv,'map_create_cancel_slam')
        ## 保存地图
        self.map_save_slam_client = self.create_client(MapSlamSrv,'map_save_slam')
        ## 地图列表
        self.map_list_client = self.create_client(MapSlamSrv,'map_list_slam')
        ## 删除地图
        self.map_delete_clinet = self.create_client(MapSlamSrv,'map_delete_slam')

        # 简单蛇形路径（对角线）
        ## 添加点
        self.add_point_pose_client = self.create_client(AddPointPose,'add_point_pose')
        ## 预览
        self.simple_snake_preview_client = self.create_client(SimpleSnakePathPreview,'simple_snake_path_preview')
        ## 保存
        self.simple_snake_save_client = self.create_client(SimpleSnakePathSave,'simple_snake_path_save')
        ## 列表
        self.simple_snake_list_client = self.create_client(SimpleSnakePathList,'simple_snake_path_list')
        ## 沿路经运动
        self.task_path_start_client = self.create_client(TaskPath,'task_path_start')
        ## 清除记录
        self.simple_snake_clear_client = self.create_client(SetBool,'simple_snake_path_clear')
        
        # 订阅IMU数据
        self.imu_subscription = self.create_subscription(
            Imu,  # 需要导入IMU消息类型
            '/imu/data_raw',#data_raw#data
            self.imu_callback,
            10
        )
        # 
        self.odom_subscription = self.create_subscription(
            Odometry,  # 需要导入odom消息类型
            '/odom_combined',
            self.odom_combined_callback,
            10
        )
        self.odom_local_subscription = self.create_subscription(
            Odometry,  # 需要导入odom消息类型
            '/odometry/local',
            self.odom_local_callback,
            10
        )

        self.fix_type_subscription = self.create_subscription(
            Int32,  # 需要导入odom消息类型
            'fix_type',
            self.fix_type_callback,
            1
        )
    
        self.current_yaw = self.create_timer(0.1, self.current_yaw_callback)
        # self.update_pose = self.create_timer(1.0,self.update_pose_callback)
    '''/******************************ROS*****************************/'''
    def cutterhead_ctl_publish_message(self, msg: bool):
        # 发布刀盘控制消息
        msg_to_publish = Bool()
        msg_to_publish.data = msg
        self.cutterhead_ctl_pub.publish(msg_to_publish)
        # self.get_logger().info(f'Publishing: "{msg_to_publish.data}"')
        # self.DisplayLog(f'Publishing: "{msg_to_publish.data}"')

    def imu_callback(self,msg):
        self.imu_orientation = {
            'x': msg.orientation.x,
            'y': msg.orientation.y,
            'z': msg.orientation.z,
            'w': msg.orientation.w
        }
        # self.DisplayLog(f"Current angle: {math.degrees(1111.000):.2f} degrees")
        
    
    def odom_combined_callback(self,msg):
        # print(msg)
        # self.x = msg.pose.pose.position.x
        # self.y = msg.pose.pose.position.y
        self.odom_combined_orientation = {
            'x': msg.pose.pose.orientation.x,
            'y': msg.pose.pose.orientation.y,
            'z': msg.pose.pose.orientation.z,
            'w': msg.pose.pose.orientation.w
        }
        # self.DisplayLog(f"Current angle: {math.degrees(22.000):.2f} degrees")
    def odom_local_callback(self,msg):
        # print(msg)
        # self.x = msg.pose.pose.position.x
        # self.y = msg.pose.pose.position.y
        self.odom_local_orientation = {
            'x': msg.pose.pose.orientation.x,
            'y': msg.pose.pose.orientation.y,
            'z': msg.pose.pose.orientation.z,
            'w': msg.pose.pose.orientation.w
        }
    
    def fix_type_callback(self,msg):
        self.fix_type = msg.data
        
    def quaternion_to_yaw(self,orientation):
 
        if orientation is None:
            return 0.0
        x = orientation['x']
        y = orientation['y']
        z = orientation['z']
        w = orientation['w']
 
        # 将四元数转换为euler欧拉角 (roll, pitch, yaw)
        (roll, pitch, yaw) = euler_from_quaternion(
            [x, y, z, w]
        )
        return yaw
    
    def update_pose_callback(self):
        try:
            # 查找从 map 到 base_link 的变换
            now = rclpy.time.Time()
            # now = self.get_clock().now()
            transform = self.tf_buffer.lookup_transform(self.map_frame,self.base_frame,now)

            # 提取平移和旋转部分
            trans = transform.transform.translation
            rot = transform.transform.rotation

            # 四元数转欧拉角，获取 yaw
            r, p, y = euler_from_quaternion([rot.x, rot.y, rot.z, rot.w])

            # 更新位姿
            self.x = trans.x
            self.y = trans.y
            self.theta = y
            # self.DisplayLog(f'[dynamic planner] {self.x}  {self.y} {self.theta}')
        except TransformException as ex:
           self.DisplayLog(f'[dynamic planner] No transform from {self.map_frame} to {self.base_frame}: {ex}')

    def current_yaw_callback(self):
        
        imu_current_angle =self.quaternion_to_yaw(self.imu_orientation) if self.imu_orientation else 0.0
        odom_combiend_current_angle =self.quaternion_to_yaw(self.odom_combined_orientation) if self.odom_combined_orientation else 0.0
        odom_local_current_angle = self.quaternion_to_yaw(self.odom_local_orientation) if self.odom_local_orientation else 0.0

        self.ui.label_13.setText(f"{math.degrees(imu_current_angle):.2f} degrees")
        self.ui.label_14.setText(f"{math.degrees(odom_combiend_current_angle):.1f} || {math.degrees(odom_local_current_angle):.1f}")
        # self.DisplayLog(f"Current angle: {math.degrees(1111.000):.2f} degrees")
        # 姿态
        self.ui.label_19.setText(f"{(self.x):.2f} m")
        self.ui.label_20.setText(f"{(self.y):.2f} m")
        self.ui.label_21.setText(f"{math.degrees(self.theta):.2f} degrees")
        
        # GPS信号强度
        self.ui.label_23.setText(f"{self.fix_type}")

    '''/******************************槽函数*****************************/'''
    '''
        调整速度
    '''
    def SetSpd(self,spdType,val):
    
        if spdType=='linear':
            self.ui.linearLCD.display(val/100.0)
            self.kb2cmd.linear_Vel=val/100.0
        else:
            self.ui.angularLCD.display(val/100.0)
            self.kb2cmd.angular_Vel=val/100.0
    
    def on_checkbox_clicked(self,state):
            if state :
                self.kb2cmd.start_listening()
            else:
                self.kb2cmd.stop_listening()
    
    # def on_pushButton_clicked(self):
    #     self.get_logger().info('button clicked')
    #     self.ui.pushButton_4.click()

    def on_pushButton_5_clicked(self):
        # 新建地图
        try:
            if not self.map_create_slam_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/map_create_slam] not available')
            
            req = MapSlamSrv.Request()
            req.operation.action = MapSlamMsg.MAP_CREATE

            # self.map_create_slam_client.call(req)
            # 异步调用服务
            future = self.map_create_slam_client.call_async(req)

            # 等待服务返回
            rclpy.spin_until_future_complete(self, future)

            # 处理响应
            if future.result() is not None:
                res = future.result()
                if res.success:
                    # self.get_logger().info('Successfully called service /map_create_slam')
                    self.DisplayLog(f'Successfully called service /map_create_slam')

                else:
                    raise Exception(f'Failed to call service /map_create_slam, error message: {res.message}')
                    # self.get_logger().error(f'Failed to call service /map_create_slam, error message: {res.message}')
            else:
                raise Exception('Service call failed or timed out')
                # self.get_logger().error('Service call failed or timed out')

        except Exception as e:
            self.DisplayLog(e)
            
    def on_pushButton_6_clicked(self):
        # 取消新建地图
        try:
            if not self.map_create_cancel_slam_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/map_create_cancel_slam] not available')
            
            req = MapSlamSrv.Request()
            req.operation.action = MapSlamMsg.MAP_CREATE_CANCEL

            future = self.map_create_cancel_slam_client.call_async(req)
            rclpy.spin_until_future_complete(self, future)
            if future.result() is not None:
                res = future.result()
                if res.success:
                    # self.get_logger().info('Successfully called service /map_create_cancel_slam_client')
                    self.DisplayLog(f'Successfully called service /map_create_cancel_slam_client')
                else:
                    # self.get_logger().error(f'Failed to call service /map_create_cancel_slam_client,error message:{(res.message)}')
                    self.DisplayLog(f'Failed to call service /map_create_cancel_slam_client, error message: {res.message}')
            else:
                raise Exception('Service call failed or timed out')
        except Exception as e:
            # self.get_logger().error(e)
            self.DisplayLog(e)

    def on_pushButton_7_clicked(self):

        # 保存新建地图
        map_name = self.ui.lineEdit.text()
        # 创建并启动新线程来处理服务调用
        # self.map_save_thread = MapSaveThread(self.map_save_slam_client, map_name)
        # self.map_save_thread.finished.connect(self.on_service_call_finished)
        # self.map_save_thread.start()
        # 保存新建地图
        try:
            map_name = self.ui.lineEdit.text()
            # self.get_logger().info(f'map name: {map_name}')

            if not self.map_save_slam_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/map_save_slam] not available')
            
            req = MapSlamSrv.Request()
            req.operation.action = MapSlamMsg.MAP_SAVE
            req.operation.map_name = map_name

            future = self.map_save_slam_client.call_async(req)
            rclpy.spin_until_future_complete(self, future)
            if future.result() is not None:
                res = future.result()
                if res.success:
                    # 显示地图
                    self.DisplayMapImage(map_name)

                    # self.get_logger().info('Successfully called service /map_save_slam_client')
                    self.DisplayLog(f'Successfully called service /map_save_slam_client')
                else:
                    # self.get_logger().error(f'Failed to call service /map_save_slam_client,error message:{format(res.message)}')
                    self.DisplayLog(f'Failed to call service /map_save_slam_client, error message: {res.message}')
            else:
                raise Exception('Service call failed or timed out')
        except Exception as e:
            # self.get_logger().error(e)
            self.DisplayLog(e)
    
    def on_activated(self):
        # 地图列表
        # self.ui.comboBox.addItems(['map1', 'map2', 'map3'])
       
        # self.DisplayLog(f'map name: {map_name}')
        # current_Items = self.ui.comboBox.currentText()
        items = [self.ui.comboBox.itemText(i) for i in range(self.ui.comboBox.count())]
        # self.ui.comboBox.clear()
        
        # for item in self.ui.comboBox.currentText():
        #     self.DisplayLog(item)

        try:
            if not self.map_list_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/map_list] not available')
            
            req = MapSlamSrv.Request()
            req.operation.action = MapSlamMsg.MAP_LIST

            # 异步调用服务
            future = self.map_list_client.call_async(req)

            # 等待服务返回
            rclpy.spin_until_future_complete(self, future)

            # 处理响应
            if future.result() is not None:
                res = future.result()
                if res.success:
                    for map_name in res.map_list:
                        if map_name not in items:
                            self.ui.comboBox.addItem(map_name)
                    # self.ui.comboBox.addItems(res.map_list)
                    self.DisplayLog(f'Successfully called service /map_list')
                else:
                    raise Exception(f'Failed to call service /map_list, error message: {res.message}')                 
            else:
                raise Exception('Service call failed or timed out')
        except Exception as e:
            self.DisplayLog(e)
    
    def on_activated_2(self):
        
        items = [self.ui.comboBox_2.itemText(i) for i in range(self.ui.comboBox_2.count())]
       
        try:
            if not self.simple_snake_list_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/simple_snake_list] not available')
            
            req = SimpleSnakePathList.Request()
            

            # 异步调用服务
            future = self.simple_snake_list_client.call_async(req)

            # 等待服务返回
            rclpy.spin_until_future_complete(self, future)

            # 处理响应
            if future.result() is not None:
                res = future.result()
                if res.success:
                    for path_name in res.path_list:
                        if path_name not in items:
                            self.ui.comboBox_2.addItem(path_name)
                    # self.ui.comboBox.addItems(res.map_list)
                    self.DisplayLog(f'Successfully called service /simple_snake_list')
                else:
                    raise Exception(f'Failed to call service /simple_snake_list, error message: {res.message}')                 
            else:
                raise Exception('Service call failed or timed out')
        except Exception as e:
            self.DisplayLog(e)
    
    def on_comboBox_currentTextChanged(self, text):

        self.DisplayLog(f'Selected map: {text}')
      
    
    def on_pushButton_12_clicked(self):
        # 删除地图

        delete_map_name = self.ui.comboBox.currentText()

        try:
            if not self.map_delete_clinet.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/map_delete] not available')
            
            req = MapSlamSrv.Request()
            req.operation.action = MapSlamMsg.MAP_DELETE
            req.operation.map_name = delete_map_name

            # 异步调用服务
            future = self.map_delete_clinet.call_async(req)

            # 等待服务返回
            rclpy.spin_until_future_complete(self, future)

            # 处理响应
            if future.result() is not None:
                res = future.result()
                if res.success:

                    # 删除下来拉框中的选项
                    self.ui.comboBox.removeItem(self.ui.comboBox.findText(delete_map_name))

                    self.DisplayLog(f'Successfully called service /map_delete')
                else:
                    raise Exception(f'Failed to call service /map_delete, error message: {res.message}')                 
            else:
                raise Exception('Service call failed or timed out')
        except Exception as e:
            self.DisplayLog(e)
    
    def on_pushButton_13_clicked(self):
        # 清空日志
        self.ui.textBrowser_3.clear()
    
    def on_pushButton_8_clicked(self):

        # 显示地图
        current_Item = self.ui.comboBox.currentText()
        self.DisplayMapImage(current_Item)

    def on_pushButton_11_clicked(self):
        # 标记点

        # 调用服务
      
        try:
            if not self.add_point_pose_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/add_point_pose] not available')
            
            req = AddPointPose.Request()
            if not self.ui.textBrowser_4.toPlainText().strip():
                # 空的文本框
                req.first_pose = True
            else:
                req.second_pose = True
            
            # 异步调用服务
            future = self.add_point_pose_client.call_async(req)

            # 等待服务返回
            rclpy.spin_until_future_complete(self, future)

            # 处理响应
            if future.result() is not None:
                res = future.result()
                if res.success:

                    pose_text = f"x={res.x:.2f}, y={res.y:.2f}"
                    if not self.ui.textBrowser_4.toPlainText().strip():
                        self.ui.textBrowser_4.setPlainText(pose_text)
                        self.DisplayLog(res.message)
                    elif not self.ui.textBrowser_5.toPlainText().strip():
                        self.ui.textBrowser_5.setPlainText(pose_text)
                        self.DisplayLog(res.message)
                    # else:
                    #     self.ui.textBrowser_4.clear()
                    #     self.ui.textBrowser_5.clear()
                    #     self.DisplayLog(res.message)

                else:
                    raise Exception(f'Failed to call service /add_point_pose, error message: {res.message}')                 
            else:
                raise Exception('Service call failed or timed out')
        except Exception as e:
            self.DisplayLog(e)

      
        # 显示
        # first_pose_text = f"x={1.1111111111111111:.6f}, y={1.1111111111111111:.6f}"
        # self.ui.textBrowser_4.setPlainText(first_pose_text)
        # 检查 QTextBrowser 中是否有内容
        # if self.ui.textBrowser_4.toPlainText().strip():  # 使用 strip() 去除空白字符
        #     print("QTextBrowser 有内容")
        #     self.ui.textBrowser_5.setPlainText("这是要显示的文本内容")
        # else:
        #     print("QTextBrowser 没有内容")
        #     self.ui.textBrowser_4.setPlainText("这是要显示的文本内容")
    def on_pushButton_16_clicked(self):
        # 清除文本框（标记点记录）
        self.ui.textBrowser_4.clear()
        self.ui.textBrowser_5.clear()
        if self.ui.graphicsView.scene() is not None:
            self.ui.graphicsView.scene().clear()
        try:
            if not self.simple_snake_clear_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/simple_snake_clear_client] not available')
            
            req = SetBool.Request()
            
            # 异步调用服务
            future = self.simple_snake_clear_client.call_async(req)

            # 等待服务返回
            rclpy.spin_until_future_complete(self, future)

            # 处理响应
            if future.result() is not None:
                res = future.result()
                if res.success:
                    self.DisplayLog(res.message)
                else:
                    raise Exception(f'Failed to call service /simple_snake_clear, error message: {res.message}')
            else:
                raise Exception('Service call failed or timed out')
        except Exception as e:
            self.DisplayLog(e)

        

    def on_pushButton_26_clicked(self):
        # 预览路径

        try:
            if not self.simple_snake_preview_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/simple_snake_preview] not available')
            
            req = SimpleSnakePathPreview.Request()
            
            # 异步调用服务
            future = self.simple_snake_preview_client.call_async(req)

            # 等待服务返回
            rclpy.spin_until_future_complete(self, future)

            # 处理响应
            if future.result() is not None:
                res = future.result()
                if res.success:
                    self.DisplayPathImage(res.cache_path)
                    # pose_text = f"x={res.x:.6f}, y={res.y:.6f}"
                    # if not self.ui.textBrowser_4.toPlainText().strip():
                    #     self.ui.textBrowser_4.setPlainText(pose_text)
                    #     self.DisplayLog(res.message)
                    # elif not self.ui.textBrowser_5.toPlainText().strip():
                    #     self.ui.textBrowser_5.setPlainText(pose_text)
                    #     self.DisplayLog(res.message)
                   
                else:
                    raise Exception(f'Failed to call service /simple_snake_preview, error message: {res.message}')                 
            else:
                raise Exception('Service call failed or timed out')
        except Exception as e:
            self.DisplayLog(e)
    
    def on_pushButton_27_clicked(self):
        # 保存路径
        # 保存新建地图
        path_name = self.ui.lineEdit_3.text()
        try:
            if not self.simple_snake_save_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/simple_snake_save] not available')
            
            req = SimpleSnakePathSave.Request()
            req.path_name = path_name
            # 异步调用服务
            future = self.simple_snake_save_client.call_async(req)

            # 等待服务返回
            rclpy.spin_until_future_complete(self, future)

            # 处理响应
            if future.result() is not None:
                res = future.result()
                if res.success:
                    self.DisplayLog(res.message)
                    
                else:
                    raise Exception(f'Failed to call service /simple_snake_save, error message: {res.message}')                 
            else:
                raise Exception('Service call failed or timed')
        except Exception as e:
            self.DisplayLog(e)

    
    def on_pushButton_14_clicked(self):
        # 沿路经运动

        path_name = self.ui.comboBox_2.currentText()

        try:
            if not self.task_path_start_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/task_path_start_client] not available')
            
            req = TaskPath.Request()
            req.path_name = path_name
            req.start_or_end = True

            # 异步调用服务
            future = self.task_path_start_client.call_async(req)

            # 等待服务返回
            rclpy.spin_until_future_complete(self, future)

            # 处理响应
            if future.result() is not None:
                res = future.result()
                if res.success:

                    self.DisplayLog(f'Successfully called service /task_path_start_client')
                else:
                    raise Exception(f'Failed to call service /task_path_start_client, error message: {res.message}')                 
            else:
                raise Exception('Service call failed or timed out')
        except Exception as e:
            self.DisplayLog(e)
    
    def on_pushButton_17_clicked(self):
        # 沿路经运动

        path_name = self.ui.comboBox_2.currentText()

        try:
            if not self.task_path_start_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/task_path_start_client] not available')
            
            req = TaskPath.Request()
            req.path_name = path_name
            req.start_or_end = False

            # 异步调用服务
            future = self.task_path_start_client.call_async(req)

            # 等待服务返回
            rclpy.spin_until_future_complete(self, future)

            # 处理响应
            if future.result() is not None:
                res = future.result()
                if res.success:

                    self.DisplayLog(f'Successfully called service /task_path_start_client')
                else:
                    raise Exception(f'Failed to call service /task_path_start_client, error message: {res.message}')                 
            else:
                raise Exception('Service call failed or timed out')
        except Exception as e:
            self.DisplayLog(e)

    def on_pushButton_15_clicked(self):
        # 预览路径

        try:
            if not self.simple_snake_preview_client.wait_for_service(timeout_sec=1.0):
                # self.get_logger().info('service not available')
                raise Exception('service [/simple_snake_preview] not available')
            
            req = SimpleSnakePathPreview.Request()
            req.path_name = self.ui.comboBox_2.currentText()
            
            # 异步调用服务
            future = self.simple_snake_preview_client.call_async(req)

            # 等待服务返回
            rclpy.spin_until_future_complete(self, future)

            # 处理响应
            if future.result() is not None:
                res = future.result()
                if res.success:
                    self.DisplayPathImage(res.cache_path)
                    # pose_text = f"x={res.x:.6f}, y={res.y:.6f}"
                    # if not self.ui.textBrowser_4.toPlainText().strip():
                    #     self.ui.textBrowser_4.setPlainText(pose_text)
                    #     self.DisplayLog(res.message)
                    # elif not self.ui.textBrowser_5.toPlainText().strip():
                    #     self.ui.textBrowser_5.setPlainText(pose_text)
                    #     self.DisplayLog(res.message)
                   
                else:
                    raise Exception(f'Failed to call service /simple_snake_preview, error message: {res.message}')                 
            else:
                raise Exception('Service call failed or timed out')
        except Exception as e:
            self.DisplayLog(e)

        
    
    '''/******************************funcation*****************************/'''
    def on_service_call_finished(self, success, message):
        if success:
            self.DisplayMapImage(self.ui.lineEdit.text())  # 显示地图
        self.DisplayLog(message)  # 显示日志

    def DisplayLog(self,log_message):
        # %Y-%m-%d %H:%M:%S
        current_time = time.strftime("%H:%M:%S", time.localtime())  # 获取当前时间并格式化为字符串
        log_entry = f"[{current_time}] {log_message}"  # 拼接时间戳和日志信息
        self.ui.textBrowser_3.append(log_entry)  # 将日志条目添加到文本浏览器控件中


    def DisplayPathImage(self,image_path):
       
        # 检查图片文件是否存在
        if not os.path.exists(image_path):
            self.DisplayLog(f"image not found: {image_path}")
            return
        
        # 获取现有的 QGraphicsScene，若没有则创建新的
        scene = self.ui.graphicsView.scene()
        if scene is None:
            scene = QGraphicsScene()
            self.ui.graphicsView.setScene(scene)  # 设置新创建的场景

        # 启动异步线程来加载图片
        self.image_loader_thread = ImageLoaderThread(image_path)
        self.image_loader_thread.imageLoaded.connect(self.on_image_loaded)  # 连接信号
        self.image_loader_thread.start()  # 启动线程

    
    def DisplayMapImage(self,map_name):
        
        # current_Item = self.ui.comboBox.currentText()
        # if not current_Item:
        #     self.DisplayLog("No map selected.")
        #     return
        if not map_name:
            self.DisplayLog("map name is empty")
            return
        
        map_png_path = os.path.join(self.map_database_path, map_name, 'map.pgm')

        # 检查图片文件是否存在
        if not os.path.exists(map_png_path):
            self.DisplayLog(f"Map image not found: {map_png_path}")
            return
        
      
        # 获取现有的 QGraphicsScene，若没有则创建新的
        scene = self.ui.graphicsView.scene()
        if scene is None:
            scene = QGraphicsScene()
            self.ui.graphicsView.setScene(scene)  # 设置新创建的场景

        # 启动异步线程来加载图片
        self.image_loader_thread = ImageLoaderThread(map_png_path)
        self.image_loader_thread.imageLoaded.connect(self.on_image_loaded)  # 连接信号
        self.image_loader_thread.start()  # 启动线程


        # try:
        #     # 加载图片
        #     pixmap = QPixmap(map_png_path)
        #     if pixmap.isNull():
        #         raise ValueError(f"Failed to load pixmap from {map_png_path}")
        
        #     # 重新创建 QGraphicsPixmapItem 以替换当前显示的图片（如果有）
        #     item = QGraphicsPixmapItem(pixmap)

        #     item.setScale(self.ui.graphicsView.width() / pixmap.width())  # 按照视图的宽度缩放

            
        #     # 清空现有场景项，并添加新的图片项
        #     scene.clear()  # 清空现有项
        #     scene.addItem(item)  # 添加新的图片项
            
        #     # 设置场景
        #     self.ui.graphicsView.setScene(scene)

        # except Exception as e:
        #     self.DisplayLog(f"Error loading map image: {e}")

    def on_image_loaded(self, pixmap, success, error_message):
        if success:
            # 如果加载成功，更新 QGraphicsView
            scene = self.ui.graphicsView.scene() or QGraphicsScene()
            scene.clear()  # 清空现有内容
            item = QGraphicsPixmapItem(pixmap)
            scene.addItem(item)
            self.ui.graphicsView.setScene(scene)
            self.apply_scaling(item)  # 应用缩放
        else:
            # 如果加载失败，显示错误信息
            self.DisplayLog(f"Error loading map image: {error_message}")


    def apply_scaling(self, item):
        # 获取 QGraphicsView 的尺寸
        view_width = self.ui.graphicsView.width()
        view_height = self.ui.graphicsView.height()
        
        # 计算适应 QGraphicsView 尺寸的缩放比例
        scale_width = view_width / item.pixmap().width()
        scale_height = view_height / item.pixmap().height()
        
        # 选择合适的缩放比例，保持图片在视图中居中
        scale_factor = min(scale_width, scale_height)
        
        # 设置缩放比例，保持图片比例不变
        item.setScale(scale_factor)


class MapSaveThread(QThread):
    # 定义信号以便将结果传回主线程
    finished = pyqtSignal(bool, str)

    def __init__(self, map_save_slam_client, map_name):
        super().__init__()
        self.map_save_slam_client = map_save_slam_client
        self.map_name = map_name

    def run(self):
        try:
            if not self.map_save_slam_client.wait_for_service(timeout_sec=1.0):
                self.finished.emit(False, "Service [/map_save_slam] not available")
                return

            req = MapSlamSrv.Request()
            req.operation.action = MapSlamMsg.MAP_SAVE
            req.operation.map_name = self.map_name

            future = self.map_save_slam_client.call_async(req)
            
            # 使用 rclpy.spin_until_future_complete 来等待结果
            rclpy.spin_until_future_complete(self, future)
            
            # 检查未来对象的结果
            if future.result() is not None:
                res = future.result()
                if res.success:
                    self.finished.emit(True, f'Successfully called service /map_save_slam_client')
                else:
                    self.finished.emit(False, f'Failed to call service /map_save_slam_client, error message: {res.message}')
            else:
                self.finished.emit(False, 'Service call failed or timed out')
        except Exception as e:
            self.finished.emit(False, str(e))


class ImageLoaderThread(QThread):
    # 定义一个信号，用于传递加载完成后的图片和是否成功的标志
    imageLoaded = pyqtSignal(QPixmap, bool, str)

    def __init__(self, map_png_path):
        super().__init__()
        self.map_png_path = map_png_path

    def run(self):
        try:
            # 加载图片
            pixmap = QPixmap(self.map_png_path)
            if pixmap.isNull():
                raise ValueError(f"Failed to load pixmap from {self.map_png_path}")
            
            # 发送信号到主线程，传递图片对象和状态
            self.imageLoaded.emit(pixmap, True, "")
        except Exception as e:
            # 如果加载失败，发送失败信息
            self.imageLoaded.emit(QPixmap(), False, str(e))







class Keyboard2CmdVel():
    '''
        后台监控键盘，然后把数据发送出去
    '''
    def __init__(self,node,ui):
        '''
            注册独立的发布器
        '''
        self.node = node  # 保存传入的ROS 2节点
        self.ui = ui

        self.alive=True
        self.cmd_Twist=Twist()
        self.linear_Vel=0.5
        self.angular_Vel=0.7

        self.cmd_vel_pub = self.node.create_publisher(Twist, '/cmd_vel', 1)
        # 创建键盘监听器
        self.listener = None  # 初始化时设置为None
        self.listener = keyboard.Listener(on_press=self.on_press, on_release=self.on_release)
        self.listener.start()
    
    def start_listening(self):
        # 如果监听器尚未启动，才启动监听器
        if self.listener is None or not self.listener.running:
            self.listener = keyboard.Listener(on_press=self.on_press, on_release=self.on_release)
            self.listener.start()
            self.node.get_logger().info("Keyboard listener started.")
        else:
            self.node.get_logger().info("Keyboard listener is already running.")

        

    def stop_listening(self):
        self.node.get_logger().info("Stopping listener...")
        self.listener.stop()
        # 等待监听器停止
        # self.listener.join()

    def on_press(self,key):
     
        # 检查按下的键是否为上下左右 - wsad
        # try:
        #     if hasattr(key, 'char') and key.char in ['w', 's', 'a', 'd']:
        #         # 判断按键
        #         if key.char=='w':
        #             self.cmd_Twist.linear.x=self.linear_Vel
                    
        #             self.ui.pushButton.setDown(True) # 设置按钮为按下状态
        #             # self.ui.pushButton.click()  # 模拟按钮点击,触发点击事件

        #         if key.char=='s':
        #             self.cmd_Twist.linear.x=-self.linear_Vel

        #             self.ui.pushButton_4.setDown(True) # 设置按钮为按下状态
        #             # self.ui.pushButton_4.click()  # 模拟按钮点击,触发点击事件
        #         if key.char=='a':
        #             self.cmd_Twist.angular.z=self.angular_Vel

        #             self.ui.pushButton_2.setDown(True) # 设置按钮为按下状态
        #             # self.ui.pushButton_2.click()  # 模拟按钮点击,触发点击事件
        #         if key.char=='d':
        #             self.cmd_Twist.angular.z=-self.angular_Vel

        #             self.ui.pushButton_3.setDown(True) # 设置按钮为按下状态
        #             # self.ui.pushButton_3.click()  # 模拟按钮点击,触发点击事件
        #         #   发布ROS话题
        #         self.cmd_vel_pub.publish(self.cmd_Twist)
        # except AttributeError:
        # # 处理其他特殊键
        #     pass

        if key in [keyboard.Key.up, keyboard.Key.down, keyboard.Key.left, keyboard.Key.right]:
            # 判断按键
            if key==keyboard.Key.up:
                self.cmd_Twist.linear.x=self.linear_Vel
                self.ui.pushButton.setDown(True) # 设置按钮为按下状态
            if key==keyboard.Key.down:
                self.cmd_Twist.linear.x=-self.linear_Vel
                self.ui.pushButton_4.setDown(True) # 设置按钮为按下状态
            if key==keyboard.Key.left:
                self.cmd_Twist.angular.z=self.angular_Vel
                self.ui.pushButton_2.setDown(True) # 设置按钮为按下状态
            if key==keyboard.Key.right:
                self.cmd_Twist.angular.z=-self.angular_Vel
                self.ui.pushButton_3.setDown(True) # 设置按钮为按下状态
            #   发布ROS话题
            self.cmd_vel_pub.publish(self.cmd_Twist)


    def on_release(self,key):
        '''
            松开按键：
        '''
        # try:
        #     if hasattr(key, 'char') and key.char in ['w', 's', 'a', 'd']:
        #         '''释放速度，转换为0'''
        #         self.cmd_Twist.angular.z=0.0
        #         self.cmd_Twist.linear.x=0.0
        #         self.cmd_vel_pub.publish(self.cmd_Twist)
            
        #         if key.char=='w':
        #             self.ui.pushButton.setDown(False)
        #         elif key.char=='s':
        #             self.ui.pushButton_4.setDown(False)
        #         elif key.char=='a':
        #             self.ui.pushButton_2.setDown(False)
        #         elif key.char=='d':
        #             self.ui.pushButton_3.setDown(False)
        #         else:
        #             pass
        # except AttributeError :
        #     pass
    
        if key in [keyboard.Key.up, keyboard.Key.down, keyboard.Key.left, keyboard.Key.right]:
            '''释放速度，转换为0'''
            self.cmd_Twist.angular.z=0.0
            self.cmd_Twist.linear.x=0.0
            self.cmd_vel_pub.publish(self.cmd_Twist)
            if key == keyboard.Key.up:
                self.ui.pushButton.setDown(False)
            elif key == keyboard.Key.down:
                self.ui.pushButton_4.setDown(False)
            elif key == keyboard.Key.left:
                self.ui.pushButton_2.setDown(False)
            elif key == keyboard.Key.right:
                self.ui.pushButton_3.setDown(False)
            else:
                pass
            


def main():
    # 初始化 ROS 2
    rclpy.init()

    # 初始化 PyQt 应用程序
    app = QApplication(sys.argv)

    ros_node = Simulator()

    # 启动 PyQt5 应用程序
    sys.exit(app.exec_())

if __name__ == '__main__':
    main()
